﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Text;
using uint8_t = System.Byte;

namespace MissionPlanner.Utilities
{
    public static class AP_GeodesicGrid
    {
        public struct neighbor_umbrella
        {
            public neighbor_umbrella(byte[] a, byte b, byte c, byte d, byte e, byte f)
            {
                components = a;
                v0_c0 = b;
                v1_c1 = c;
                v2_c1 = d;
                v4_c4 = e;
                v0_c4 = f;
            }

            /**
             * The umbrella's components. The value of #components[i] is the
             * icosahedron triangle index of the i-th component.
             *
             * In order to find the components for T_10, the following just finding
             * the index of the opposite triangle is enough. In other words,
             * (#components[i] + 10) % 20.
             */
            public uint8_t[] components; //[5];

            /**
                * The fields with name in the format vi_cj are interpreted as the
                * following: vi_cj is the index of the vector, in the icosahedron
                * triangle pointed by #components[j], that matches the umbrella's i-th
                * vertex.
                *
                * The values don't change for T_10.
                */
            public uint8_t v0_c0;

            public uint8_t v1_c1;
            public uint8_t v2_c1;
            public uint8_t v4_c4;
            public uint8_t v0_c4;
        };

        /*
 * Copyright (C) 2016  Intel Corporation. All rights reserved.
 *
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

        /*
         * This comment section explains the basic idea behind the implementation.
         *
         * Vectors difference notation
         * ===========================
         * Let v and w be vectors. For readability purposes, unless explicitly
         * otherwise noted, the notation vw will be used to represent w - v.
         *
         * Relationship between a vector and a triangle in 3d space
         * ========================================================
         * Vector in the area of a triangle
         * --------------------------------
         * Let T = (a, b, c) be a triangle, where a, b and c are also vectors and
         * linearly independent. A vector inside that triangle can be written as one of
         * its vertices plus the sum of the positively scaled vectors from that vertex
         * to the other ones. Taking a as the first vertex, a vector p in the area
         * formed by T can be written as:
         *
         *     p = a + w_ab * ab + w_ac * ac
         *
         * It's fairly easy to see that if p is in the area formed by T, then w_ab >= 0
         * and w_ac >= 0. That vector p can also be written as:
         *
         *     p = b + w_ba * ba + w_bc * bc
         *
         * It's easy to check that the triangle formed by (a + w_ab * ab, b + w_ba *
         * ba, p) is similar to T and, with the correct algebraic manipulations, we can
         * come to the conclusion that:
         *
         *     w_ba = 1 - w_ab - w_ac
         *
         * Since we know that w_ba >= 0, then w_ab + w_ac <= 1. Thus:
         *
         *     ----------------------------------------------------------
         *     | p = a + w_ab * ab + w_ac * ac is in the area of T iff: |
         *     | w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1           |
         *     ----------------------------------------------------------
         *
         * Proving backwards shouldn't be difficult.
         *
         * Vector p can also be written as:
         *
         *     p = (1 - w_ab - w_ba) * a + w_ab * b + w_ba * c
         *
         *
         * Vector that crosses a triangle
         * ------------------------------
         * Let T be the same triangle discussed above and let v be a vector such that:
         *
         *     v = x * a + y * b + z * c
         *     where x >= 0, y >= 0, z >= 0, and x + y + z > 0.
         *
         * It's geometrically easy to see that v crosses the triangle T. But that can
         * also be verified analytically.
         *
         * The vector v crosses the triangle T iff there's a positive alpha such that
         * alpha * v is in the area formed by T, so we need to prove that such value
         * exists. To find alpha, we solve the equation alpha * v = p, which will lead
         * us to the system, for the variables alpha, w_ab and w_ac:
         *
         *    alpha * x = 1 - w_ab - w_ac
         *    alpha * y = w_ab
         *    alpha * z = w_ac,
         *    where w_ab >= 0 and w_ac >= 0 and w_ab + w_ac <= 1
         *
         * That will lead to alpha = 1 / (x + y + z), w_ab = y / (x + y + b) and
         * w_ac = z / (x + y + z) and the following holds:
         *  - alpha does exist because x + y + z > 0.
         *  - w_ab >= 0 and w_ac >= 0 because y >= 0 and z >= 0 and x + y + z > 0.
         *  - 0 <= 1 - w_ab - w_ac <= 1 because 0 <= (y + z) / (x + y + z) <= 1.
         *
         * Thus:
         *
         *     ----------------------------------------------------------
         *     | v = x * a + y * b + z * c crosses T = (a, b, c), where |
         *     | a, b and c are linearly independent, iff:              |
         *     | x >= 0, y >= 0, z >= 0 and x + y + z > 0               |
         *     ----------------------------------------------------------
         *
         * Moreover:
         *  - if one of the coefficients is zero, then v crosses the edge formed by the
         *  vertices multiplied by the non-zero coefficients.
         *  - if two of the coefficients are zero, then v crosses the vertex multiplied
         *  by the non-zero coefficient.
         */

        /* This was generated with
         * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
       static private neighbor_umbrella[] _neighbor_umbrellas =
        {
            new neighbor_umbrella(new byte[] {9, 8, 7, 12, 14}, 1, 2, 0, 0, 2),
            new neighbor_umbrella(new byte[] {1, 2, 4, 5, 3}, 0, 0, 2, 2, 0),
            new neighbor_umbrella(new byte[] {16, 15, 13, 18, 17}, 2, 2, 0, 2, 1),
        };

        /* This was generated with
         * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
        static private Matrix3[] _inverses =
        {
            new Matrix3(-0.309017f, 0.500000f, 0.190983f,
                 0.000000f, 0.000000f, -0.618034f,
                -0.309017f, -0.500000f, 0.190983f),
            new Matrix3(-0.190983f, 0.309017f, -0.500000f,
                -0.500000f, -0.190983f, 0.309017f,
                 0.309017f, -0.500000f, -0.190983f),
            new Matrix3(-0.618034f, 0.000000f, 0.000000f,
                 0.190983f, -0.309017f, -0.500000f,
                 0.190983f, -0.309017f, 0.500000f),
            new Matrix3(-0.500000f, 0.190983f, -0.309017f,
                 0.000000f, -0.618034f, 0.000000f,
                 0.500000f, 0.190983f, -0.309017f),
            new Matrix3(-0.190983f, -0.309017f, -0.500000f,
                -0.190983f, -0.309017f, 0.500000f,
                 0.618034f, 0.000000f, 0.000000f),
            new Matrix3(-0.309017f, -0.500000f, -0.190983f,
                 0.190983f, 0.309017f, -0.500000f,
                 0.500000f, -0.190983f, 0.309017f),
            new Matrix3(0.309017f, -0.500000f, 0.190983f,
                 0.000000f, 0.000000f, -0.618034f,
                 0.309017f, 0.500000f, 0.190983f),
            new Matrix3(0.190983f, -0.309017f, -0.500000f,
                 0.500000f, 0.190983f, 0.309017f,
                -0.309017f, 0.500000f, -0.190983f),
            new Matrix3(0.500000f, -0.190983f, -0.309017f,
                 0.000000f, 0.618034f, 0.000000f,
                -0.500000f, -0.190983f, -0.309017f),
            new Matrix3(0.309017f, 0.500000f, -0.190983f,
                -0.500000f, 0.190983f, 0.309017f,
                -0.190983f, -0.309017f, -0.500000f)
        };

        /* This was generated with
         * libraries/AP_Math/tools/geodesic_grid/geodesic_grid.py */
        static private Matrix3[] _mid_inverses =
        {
            new Matrix3(-0.000000f, 1.000000f, -0.618034f,
                 0.000000f, -1.000000f, -0.618034f,
                 -0.618034f, 0.000000f, 1.000000f),
            new Matrix3(-1.000000f, 0.618034f, -0.000000f,
                 -0.000000f, -1.000000f, 0.618034f,
                 0.618034f, -0.000000f, -1.000000f),
            new Matrix3(-0.618034f, -0.000000f, -1.000000f,
                1.000000f, -0.618034f, -0.000000f,
                -0.618034f, 0.000000f, 1.000000f),
            new Matrix3(-1.000000f, -0.618034f, -0.000000f,
                1.000000f, -0.618034f, 0.000000f,
                -0.000000f, 1.000000f, -0.618034f),
            new Matrix3(-1.000000f, -0.618034f, 0.000000f,
                0.618034f, 0.000000f, 1.000000f,
                0.618034f, 0.000000f, -1.000000f),
            new Matrix3(-0.618034f, -0.000000f, -1.000000f,
                1.000000f, 0.618034f, -0.000000f,
                0.000000f, -1.000000f, 0.618034f),
            new Matrix3(0.000000f, -1.000000f, -0.618034f,
                0.000000f, 1.000000f, -0.618034f,
                0.618034f, -0.000000f, 1.000000f),
            new Matrix3(1.000000f, -0.618034f, -0.000000f,
                0.000000f, 1.000000f, 0.618034f,
                -0.618034f, 0.000000f, -1.000000f),
            new Matrix3(1.000000f, 0.618034f, -0.000000f,
                -1.000000f, 0.618034f, 0.000000f,
                0.000000f, -1.000000f, -0.618034f),
            new Matrix3(-0.000000f, 1.000000f, 0.618034f,
                -1.000000f, -0.618034f, -0.000000f,
                0.618034f, 0.000000f, -1.000000f),
        };

        public static int section(Vector3 v, bool inclusive)
        {
            int i = _triangle_index(v, inclusive);
            if (i < 0)
            {
                return -1;
            }

            int j = _subtriangle_index(i, v, inclusive);
            if (j < 0)
            {
                return -1;
            }

            return 4 * i + j;
        }

        static int _neighbor_umbrella_component(int idx, int comp_idx)
        {
            if (idx < 3)
            {
                return _neighbor_umbrellas[idx].components[comp_idx];
            }
            return (_neighbor_umbrellas[idx % 3].components[comp_idx] + 10) % 20;
        }

        static int _from_neighbor_umbrella(int idx,
            ref Vector3 v,
            ref Vector3 u,
            bool inclusive)
        {
            /* The following comparisons between the umbrella's first and second
             * vertices' coefficients work for this algorithm because all vertices'
             * vectors are of the same length. */

            if (is_equal(u.x, u.y))
            {
                /* If the coefficients of the first and second vertices are equal, then
                 * v crosses the first component or the edge formed by the umbrella's
                 * pivot and forth vertex. */
                int comp = _neighbor_umbrella_component(idx, 0);
                var w = _inverses[comp % 10] * v;
                if (comp > 9)
                {
                    w = -w;
                }
                float x0 = (float) w[_neighbor_umbrellas[idx % 3].v0_c0];
                if (is_zero(x0))
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return comp;
                }
                else if (x0 < 0)
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return _neighbor_umbrella_component(idx, u.x < u.y ? 3 : 2);
                }

                return comp;
            }

            if (u.y > u.x)
            {
                /* If the coefficient of the second vertex is greater than the first
                 * one's, then v crosses the first, second or third component. */
                int comp = _neighbor_umbrella_component(idx, 1);
                var w = _inverses[comp % 10] * v;
                if (comp > 9)
                {
                    w = -w;
                }
                float x1 = (float) w[_neighbor_umbrellas[idx % 3].v1_c1];
                float x2 = (float) w[_neighbor_umbrellas[idx % 3].v2_c1];

                if (is_zero(x1))
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return _neighbor_umbrella_component(idx, x1 < 0 ? 2 : 1);
                }
                else if (x1 < 0)
                {
                    return _neighbor_umbrella_component(idx, 2);
                }

                if (is_zero(x2))
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return _neighbor_umbrella_component(idx, x2 > 0 ? 1 : 0);
                }
                else if (x2 < 0)
                {
                    return _neighbor_umbrella_component(idx, 0);
                }

                return comp;
            }
            else
            {
                /* If the coefficient of the second vertex is lesser than the first
                 * one's, then v crosses the first, fourth or fifth component. */
                int comp = _neighbor_umbrella_component(idx, 4);
                var w = _inverses[comp % 10] * v;
                if (comp > 9)
                {
                    w = -w;
                }
                float x4 = (float) w[_neighbor_umbrellas[idx % 3].v4_c4];
                float x0 = (float) w[_neighbor_umbrellas[idx % 3].v0_c4];

                if (is_zero(x4))
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return _neighbor_umbrella_component(idx, x4 < 0 ? 0 : 4);
                }
                else if (x4 < 0)
                {
                    return _neighbor_umbrella_component(idx, 0);
                }

                if (is_zero(x0))
                {
                    if (!inclusive)
                    {
                        return -1;
                    }
                    return _neighbor_umbrella_component(idx, x0 > 0 ? 4 : 3);
                }
                else if (x0 < 0)
                {
                    return _neighbor_umbrella_component(idx, 3);
                }

                return comp;
            }
        }

        static private bool is_zero(double x0)
        {
            return x0 == 0;
        }

        static private bool is_equal(double x, double y)
        {
            return x == y;
        }

        static int _triangle_index(Vector3 v, bool inclusive)
        {
            /* w holds the coordinates of v with respect to the basis comprised by the
             * vectors of T_i */
            var w = _inverses[0] * v;
            int zero_count = 0;
            int balance = 0;
            int umbrella = -1;

            if (is_zero(w.x))
            {
                zero_count++;
            }
            else if (w.x > 0)
            {
                balance++;
            }
            else
            {
                balance--;
            }

            if (is_zero(w.y))
            {
                zero_count++;
            }
            else if (w.y > 0)
            {
                balance++;
            }
            else
            {
                balance--;
            }

            if (is_zero(w.z))
            {
                zero_count++;
            }
            else if (w.z > 0)
            {
                balance++;
            }
            else
            {
                balance--;
            }

            switch (balance)
            {
                case 3:
                    /* All coefficients are positive, thus return the first triangle. */
                    return 0;
                case -3:
                    /* All coefficients are negative, which means that the coefficients for
                     * -w are positive, thus return the first triangle's opposite. */
                    return 10;
                case 2:
                    /* Two coefficients are positive and one is zero, thus v crosses one of
                     * the edges of the first triangle. */
                    return inclusive ? 0 : -1;
                case -2:
                    /* Analogous to the previous case, but for the opposite of the first
                     * triangle. */
                    return inclusive ? 10 : -1;
                case 1:
                    /* There are two possible cases when balance is 1:
                     *
                     * 1) Two coefficients are zero, which means v crosses one of the
                     * vertices of the first triangle.
                     *
                     * 2) Two coefficients are positive and one is negative. Let a and b be
                     * vertices with positive coefficients and c the one with the negative
                     * coefficient. That means that v crosses the triangle formed by a, b
                     * and -c. The vector -c happens to be the 3-th vertex, with respect to
                     * (a, b), of the first triangle's neighbor umbrella with respect to a
                     * and b. Thus, v crosses one of the components of that umbrella. */
                    if (zero_count == 2)
                    {
                        return inclusive ? 0 : -1;
                    }

                    if (!is_zero(w.x) && w.x < 0)
                    {
                        umbrella = 1;
                    }
                    else if (!is_zero(w.y) && w.y < 0)
                    {
                        umbrella = 2;
                    }
                    else
                    {
                        umbrella = 0;
                    }

                    break;
                case -1:
                    /* Analogous to the previous case, but for the opposite of the first
                     * triangle. */
                    if (zero_count == 2)
                    {
                        return inclusive ? 10 : -1;
                    }

                    if (!is_zero(w.x) && w.x > 0)
                    {
                        umbrella = 4;
                    }
                    else if (!is_zero(w.y) && w.y > 0)
                    {
                        umbrella = 5;
                    }
                    else
                    {
                        umbrella = 3;
                    }
                    w = -w;

                    break;
                case 0:
                    /* There are two possible cases when balance is 1:
                     *
                     * 1) The vector v is the null vector, which doesn't cross any section.
                     *
                     * 2) One coefficient is zero, another is positive and yet another is
                     * negative. Let a, b and c be the respective vertices for those
                     * coefficients, then the statements in case (2) for when balance is 1
                     * are also valid here.
                     */
                    if (zero_count == 3)
                    {
                        return -1;
                    }

                    if (!is_zero(w.x) && w.x < 0)
                    {
                        umbrella = 1;
                    }
                    else if (!is_zero(w.y) && w.y < 0)
                    {
                        umbrella = 2;
                    }
                    else
                    {
                        umbrella = 0;
                    }

                    break;
            }

            //assert(umbrella >= 0);

            switch (umbrella % 3)
            {
                case 0:
                    w.z = -w.z;
                    break;
                case 1:
                    w = new Vector3(w.y, w.z, -w.x);
                    break;
                case 2:
                    w = new Vector3(w.z, w.x, -w.y);
                    break;
            }

            return _from_neighbor_umbrella(umbrella, ref v, ref w, inclusive);
        }

        static int _subtriangle_index(int triangle_index,
            Vector3 v,
            bool inclusive)
        {
            /* w holds the coordinates of v with respect to the basis comprised by the
             * vectors of the middle triangle of T_i where i is triangle_index */
            var w = _mid_inverses[triangle_index % 10] * v;
            if (triangle_index > 9)
            {
                w = -w;
            }

            if ((is_zero(w.x) || is_zero(w.y) || is_zero(w.z)) && !inclusive)
            {
                return -1;
            }

            /* At this point, we know that v crosses the icosahedron triangle pointed
             * by triangle_index. Thus, we can geometrically see that if v doesn't
             * cross its middle triangle, then one of the coefficients will be negative
             * and the other ones positive. Let a and b be the non-negative
             * coefficients and c the negative one. In that case, v will cross the
             * triangle with vertices (a, b, -c). Since we know that v crosses the
             * icosahedron triangle and the only sub-triangle that contains the set of
             * points (seen as vectors) that cross the triangle (a, b, -c) is the
             * middle triangle's neighbor with respect to a and b, then that
             * sub-triangle is the one crossed by v. */
            if (!is_zero(w.x) && w.x < 0)
            {
                return 3;
            }
            if (!is_zero(w.y) && w.y < 0)
            {
                return 1;
            }
            if (!is_zero(w.z) && w.z < 0)
            {
                return 2;
            }

            /* If x >= 0 and y >= 0 and z >= 0, then v crosses the middle triangle. */
            return 0;
        }

    }
}